﻿using System;
using UnityEngine;

namespace NinjaToolbox.PhysicsSystem
{
    [Serializable]
    public class SphereShape : IPhysicsShape
    {
        [HideInInspector]
        public Matrix4x4 matrix;
        public Vector3 offset;
        public float radius;

        public float ConcertRadius
        {
            get
            {
                var lossyScale = matrix.lossyScale;
                var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));

                return maxScale * radius;
            }
        }

        public Bounds AABB { get { return new Bounds(GetWorldCenter(), Vector3.one * ConcertRadius); } }


        public Vector3 GetWorldCenter()
        {
            return matrix.MultiplyPoint3x4(offset);
        }

        public bool IsIntersects(IPhysicsShape other)
        {
            var boxShape = other as BoxShape;
            if (boxShape != null)
            {
                if (!AABB.Intersects(boxShape.AABB)) return false;
                return PhysicsUtil.BoxVSSphere(boxShape.matrix, boxShape.offset, boxShape.size, GetWorldCenter(), ConcertRadius);
            }

            var sphereShape = other as SphereShape;
            if (sphereShape != null)
            {
                return PhysicsUtil.SphereVSSphere(GetWorldCenter(), ConcertRadius, sphereShape.GetWorldCenter(), sphereShape.ConcertRadius);
            }

            var virtualCapsuleShape = other as VirtualCapsuleShape;
            if (virtualCapsuleShape != null)
            {
                if (!AABB.Intersects(virtualCapsuleShape.AABB)) return false;
                return PhysicsUtil.SphereVSVirtualCapsule(GetWorldCenter(), ConcertRadius, virtualCapsuleShape.matrix, virtualCapsuleShape.offset, virtualCapsuleShape.height, virtualCapsuleShape.radius, virtualCapsuleShape.direction, virtualCapsuleShape.virtualSegment);
            }

            return false;
        }

        public bool IsIntersects(Vector3 lineBegin, Vector3 lineEnd)
        {
            var direction = lineEnd - lineBegin;
            var distance = direction.magnitude;
            direction = direction.normalized;
            var ray = new Ray(lineBegin, direction);
            var intersectRayDistance = 0f;
            if (AABB.IntersectRay(ray, out intersectRayDistance)
                && intersectRayDistance > distance)
            {
                return false;
            }

            var temp = Vector3.zero;
            if (BetweenLineAndSphere(GetWorldCenter(), radius, lineBegin, lineEnd, out temp, out temp))
            {
                return true;
            }

            return false;
        }

        public bool IsContainPoint(Vector3 point)
        {
            return Vector3.Distance(GetWorldCenter(), point) <= radius;
        }

        public Vector3 MassPointToEdge(Vector3 massPoint, Vector3 outsidePoint)
        {
            return massPoint + (outsidePoint - massPoint).normalized * radius;
        }

        public Vector3 GetEdgeNormal(Vector3 massPoint, Vector3 outsidePoint)
        {
            return (outsidePoint - massPoint).normalized;
        }

        public void DrawGizmos(Color color)
        {
            var cacheColor = Gizmos.color;

            Gizmos.color = color;

            Gizmos.DrawWireSphere(GetWorldCenter(), ConcertRadius);

            Gizmos.color = cacheColor;
        }

        bool BetweenLineAndSphere(
            Vector3 sphereCenter, float sphereRadius,
            Vector3 point1, Vector3 point2,
            out Vector3 intersection1, out Vector3 intersection2)
        {
            float t;

            var dx = point2.x - point1.x;
            var dy = point2.y - point1.y;
            var dz = point2.z - point1.z;

            var a = dx * dx + dy * dy + dz * dz;
            var b = 2 * (dx * (point1.x - sphereCenter.x) + dy * (point1.y - sphereCenter.y) + dz * (point1.z - sphereCenter.z));
            var c = (point1.x - sphereCenter.x) * (point1.x - sphereCenter.x)
                + (point1.y - sphereCenter.y) * (point1.y - sphereCenter.y)
                + (point1.z - sphereCenter.z) * (point1.z - sphereCenter.z) - sphereRadius * sphereRadius;

            var determinate = b * b - 4 * a * c;

            // Two solutions.
            t = (-b + Mathf.Sqrt(determinate)) / (2 * a);
            intersection1 = new Vector3(point1.x + t * dx, point1.y + t * dy, point1.z + t * dz);
            t = (-b - Mathf.Sqrt(determinate)) / (2 * a);
            intersection2 = new Vector3(point1.x + t * dx, point1.y + t * dy, point1.z + t * dz);

            if (intersection1.normalized == Vector3.zero && intersection2.normalized == Vector3.zero)
                return false;
            else
                return true;
        }

        Matrix4x4 IPhysicsShape.Matrix { get { return matrix; } set { matrix = value; } }
    }
}
